#include "bsp_can.h"
#include "driver_chassis.h"
#include "driver_gimbal.h"
#include "driver_superc.h"

uint8_t CAN1_BUFF[8] = {0};
uint8_t CAN2_BUFF[8] = {0};
CAN_RxHeaderTypeDef CAN1_RxMsg;
CAN_RxHeaderTypeDef CAN2_RxMsg;
float VXINIT,VYINIT=0;

void CAN1_Filter_Init(void)
{
    CAN_FilterTypeDef CAN1_FilterInitStructure;

    CAN1_FilterInitStructure.FilterActivation = ENABLE;
    CAN1_FilterInitStructure.FilterFIFOAssignment = 0;
    CAN1_FilterInitStructure.FilterScale = CAN_FILTERSCALE_32BIT;
    CAN1_FilterInitStructure.FilterMode = CAN_FILTERMODE_IDMASK;
    CAN1_FilterInitStructure.FilterIdLow = 0x0000;
    CAN1_FilterInitStructure.FilterIdHigh = 0x0000;
    CAN1_FilterInitStructure.FilterMaskIdLow = 0x0000;
    CAN1_FilterInitStructure.FilterMaskIdHigh = 0x0000;
    CAN1_FilterInitStructure.SlaveStartFilterBank = 14;
    CAN1_FilterInitStructure.FilterBank = 0;

    if(HAL_CAN_ConfigFilter(&hcan1, &CAN1_FilterInitStructure) != HAL_OK)
        Error_Handler();
    HAL_CAN_Start(&hcan1);
    HAL_CAN_ActivateNotification(&hcan1,CAN_IT_RX_FIFO0_MSG_PENDING);
}

void CAN2_Filter_Init(void)
{
    CAN_FilterTypeDef CAN2_FilterInitStructure;

    CAN2_FilterInitStructure.FilterActivation = ENABLE;
    CAN2_FilterInitStructure.FilterFIFOAssignment = 0;
    CAN2_FilterInitStructure.FilterScale = CAN_FILTERSCALE_32BIT;
    CAN2_FilterInitStructure.FilterMode = CAN_FILTERMODE_IDMASK;
    CAN2_FilterInitStructure.FilterIdHigh = 0x0000;
    CAN2_FilterInitStructure.FilterIdLow = 0x0000;
    CAN2_FilterInitStructure.FilterMaskIdLow = 0x0000;
    CAN2_FilterInitStructure.FilterMaskIdHigh = 0x0000;
    CAN2_FilterInitStructure.SlaveStartFilterBank = 14;
    CAN2_FilterInitStructure.FilterBank = 14;

    if(HAL_CAN_ConfigFilter(&hcan2, &CAN2_FilterInitStructure) != HAL_OK)
        Error_Handler();
    HAL_CAN_Start(&hcan2);
    HAL_CAN_ActivateNotification(&hcan2,CAN_IT_RX_FIFO0_MSG_PENDING);
}


extern int PowerUpFlag;
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    if(hcan->Instance == hcan1.Instance)
    {
        if(HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &CAN1_RxMsg, CAN1_BUFF) == HAL_OK)
        {
            //收到数据来自电机
            if(CAN1_RxMsg.StdId >= 0x201 && CAN1_RxMsg.StdId < 0x205)
            {
                uint16_t Error=GetErrorState();

                //当四个电机都失联时证明底盘下电
                if(((Error>>CHASSIS_MOTOR_0)&1) && ((Error>>CHASSIS_MOTOR_1)&1)
                    && ((Error>>CHASSIS_MOTOR_2)&1) && ((Error>>CHASSIS_MOTOR_3)&1))
                {
                    PowerUpFlag=1;
                }

                LostCounterFeed(CAN1_RxMsg.StdId - 0x201 + CHASSIS_MOTOR_0);

                if(PowerUpFlag==0)
                {
                    //上电检测
                    PowerUpFlag=1;
                }

                for(int i = 0; i < 8; i++)
                {
                    ChassisMotor[CAN1_RxMsg.StdId - 0x201].CANMessege[i] = CAN1_BUFF[i];
                }
            }
        }
    }
    else if(hcan->Instance == hcan2.Instance)
    {
        if(HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO0, &CAN2_RxMsg, CAN2_BUFF) == HAL_OK){
            switch (CAN2_RxMsg.StdId)
            {
                //yaw电机数据
                //要改成DM4310，发0x02,收0x206
                case 0x205:
                {
                    LostCounterFeed(GIMBAL_MOTOR_YAW);
                    for (int i = 0; i < 8; i++) {
                        YawMotor.CANMessege[i] = CAN2_BUFF[i];
                    }
                    break;
                }

                //云台发的数据1
                case 0x211:
                {
                    //数据解包
                    GimbalReceive.autoSuperFlag = (CAN2_BUFF[4] >> 7) & 1; //开自动电容
                    GimbalReceive.SpinFlag = (CAN2_BUFF[4] >> 6) & 1;
                    GimbalReceive.CanWatchDog = (CAN2_BUFF[4] >> 5) & 1;
                    GimbalReceive.LockFlag = (CAN2_BUFF[4] >> 4) & 1; //自锁
                    GimbalReceive.keyboardFlag = (CAN2_BUFF[4] >> 3) & 1;
                    GimbalReceive.mcl = (CAN2_BUFF[4] >> 2) & 1;
                    GimbalReceive.rampC = (CAN2_BUFF[4] >> 1) & 1; //飞坡电容

                    //跟随模式
                    if(!GimbalReceive.SpinFlag && !GimbalReceive.LockFlag)
                        GimbalReceive.FollowFlag = 1;
                    else
                        GimbalReceive.FollowFlag = 0;
                    GimbalReceive.VisionMode = CAN2_BUFF[6];

                    GimbalReceive.autoAim  = !(CAN2_BUFF[6] & 7);
                    if((CAN2_BUFF[6] & 3)==1)
                    {
                        GimbalReceive.symbal_s=1;
                        GimbalReceive.symbal_b=0;
                    }
                    else if((CAN2_BUFF[6] & 3)==3)
                    {
                        GimbalReceive.symbal_s=0;
                        GimbalReceive.symbal_b=1;
                    }
                    else
                    {
                        GimbalReceive.symbal_s=0;
                        GimbalReceive.symbal_b=0;
                    }


                    GimbalReceive.Error = CAN2_BUFF[7];

                    if (!(CAN2_BUFF[7] & 0x01))//No OverSpeed
                        LostCounterFeed(OVER_SPEED_LOST);
                    if (!((CAN2_BUFF[7] & 0x02) >> 1))//No OverHeat
                        LostCounterFeed(OVER_HEAT_LOST);
                    if (!((CAN2_BUFF[7] & 0x04) >> 2))//FeedMotor
                        LostCounterFeed(FEEDMOTOR_LOST_COUNT);
                    if (!((CAN2_BUFF[7] & 0x08) >> 3))//Friction
                        LostCounterFeed(FRICTION_LOST_COUNT);
                    if (!((CAN2_BUFF[7] & 0x10) >> 4))//Pitch
                        LostCounterFeed(GIMBAL_MOTOR_PITCH);
                    if (!((CAN2_BUFF[7] & 0x20) >> 5))//Vision
                        LostCounterFeed(VISION_LOST_COUNT);

                    GimbalReceive.upHillFlag = (CAN2_BUFF[7] & 0x40) >> 6;
                    GimbalReceive.rampFlag = (CAN2_BUFF[7] & 0x80) >> 7;
                    GimbalReceive.tunnel = CAN2_BUFF[5];

                    //X，Y移动速度
                    VXINIT = (float) ((CAN2_BUFF[0] << 8) | (CAN2_BUFF[1] & 0xFF));
                    VYINIT = (float) ((CAN2_BUFF[2] << 8) | (CAN2_BUFF[3] & 0xFF));
                    int lostnum = 0;
                    for (int i = 0; i < 8; i++)
                        if (CAN2_BUFF[i] == 0)
                            lostnum++;
                    if (lostnum == 8)
                        GimbalReceive.LostCounter = 1;//关控保护
                    else
                        GimbalReceive.LostCounter = 0;

#if infantryID==1
                    if (VXINIT > 363.f && VXINIT < 1685.f)
                        GimbalReceive.SetSpeedX = -(VXINIT - 1024.f) / 660.f;
                    if (VYINIT > 363.f && VYINIT < 1685.f)
                        GimbalReceive.SetSpeedY = -(VYINIT - 1024.f) / 660.f;
#elif infantryID==0
                    if (VXINIT > 363.f && VXINIT < 1685.f)
                        GimbalReceive.SetSpeedX = -(VXINIT - 1024.f) / 660.f;
                    if (VYINIT > 363.f && VYINIT < 1685.f)
                        GimbalReceive.SetSpeedY = -(VYINIT - 1024.f) / 660.f;
#endif
                    //关控时候速度置为零
                    if (GimbalReceive.LostCounter)
                    {
                        GimbalReceive.SetSpeedX = 0;
                        GimbalReceive.SetSpeedY = 0;
                    }

                    if(GimbalReceive.LostCounter == 0)
                    {
                        LostCounterFeed(REMOTE_LOST_COUNT);
                    }
                    break;
                }
                //云台发的数据2
                case 0x212:
                {
                    //目标位置信息
                    GimbalReceive.armFlag = CAN2_BUFF[0];
                    GimbalReceive.target_location_y= (int16_t)(((CAN2_BUFF[2] << 8) | (CAN2_BUFF[3] & 0xFF)) / 10);
                    GimbalReceive.target_location_z= (uint16_t)(((CAN2_BUFF[4] << 8) | (CAN2_BUFF[5] & 0xFF)) / 10);
                    GimbalReceive.heat = (int16_t)((CAN2_BUFF[6] << 8) | (CAN2_BUFF[7] & 0xFF));
                    break;
                }

                //超级电容数据
                case 0x311:
                {
                    SC_recv_message(CAN2_BUFF);
                    //超级电容不处于ERROR或DEBUG状态时，喂狗
                    if(CAN2_BUFF[6] != SUPERC_ERROR && CAN2_BUFF[6] != SUPERC_DEBUG)LostCounterFeed(SUPERC_LOST_COUNT);
                    break;
                }
                default:
                    break;
            }
        }
    }
}


//两个发送函数：CAN1_Send_Msg仅底盘CAN1在用，发给4个M3508
//           CAN2_Send_Msg用于底盘发给云台
uint8_t CAN1_Send_Msg(uint8_t* msg, uint8_t len, uint32_t id)
{
    CAN_TxHeaderTypeDef CAN1_TxHeader;
    uint8_t i;
    uint8_t CAN1_BUFF[8];
    CAN1_TxHeader.StdId = id;
    CAN1_TxHeader.ExtId = 0x12;
    CAN1_TxHeader.IDE = CAN_ID_STD;
    CAN1_TxHeader.RTR = CAN_RTR_DATA;
    CAN1_TxHeader.DLC = len;
    for (i = 0; i < len; i++)
        CAN1_BUFF[i] = msg[i];
    if (HAL_CAN_AddTxMessage(&hcan1, &CAN1_TxHeader, CAN1_BUFF, (uint32_t*)CAN_TX_MAILBOX0) != HAL_OK) return 1;
    return 0;
}

uint8_t CAN2_Send_Msg(uint8_t* msg, uint8_t len, uint32_t id)
{
    CAN_TxHeaderTypeDef CAN2_TxHeader;
    uint8_t i;
    uint8_t CAN2_SEND_BUFF[8];
    CAN2_TxHeader.StdId = id;
    CAN2_TxHeader.ExtId = 0x12;
    CAN2_TxHeader.IDE = CAN_ID_STD;
    CAN2_TxHeader.RTR = CAN_RTR_DATA;
    CAN2_TxHeader.DLC = len;
    for (i = 0; i < len; i++)
        CAN2_SEND_BUFF[i] = msg[i];
    if (HAL_CAN_AddTxMessage(&hcan2, &CAN2_TxHeader, CAN2_SEND_BUFF, (uint32_t*)CAN_TX_MAILBOX0) != HAL_OK) return 1;
    return 0;
}
